#include "FTE2000.h"

static FTE2000_FLASH_MEMMAP Ftflash = {0};

static FTE2000_FLASH_MEMMAP* get_VirtAddr_from_context(const struct flashctx *flash)
{
	if (!flash || !flash->mst || !flash->mst->spi.data) {
		msg_perr("Unable to extract Virt Addr from flash context.\n");
		return NULL;
	}
	return flash->mst->spi.data;
}

static int Ft_spi_send_command(const struct flashctx *flash,
				unsigned int writecnt, unsigned int readcnt,
				const unsigned char *writearr,
				unsigned char *readarr)
{
    uint32_t cmd;
    uint32_t Addr;
    uint32_t read[2];
    uint32_t write[2] = {0};
    uint32_t Num = 0;
    uint32_t i = 0;

	if (writecnt > 9 || readcnt > 8 || writecnt == 0) {
		msg_perr("%s: Invalid read/write count for send command.\n",
                         __func__);
		return SPI_GENERIC_ERROR;
	}

	FTE2000_FLASH_MEMMAP *pVirtAddr = get_VirtAddr_from_context(flash);
    if (NULL == pVirtAddr)
		return SPI_GENERIC_ERROR;

    if ((ERASE_4 == writearr[0]) || (ERASE_32 == writearr[0]) || (ERASE_64 == writearr[0]) || (ERASE_D7 == writearr[0]) ||
        (ERASE_C4 == writearr[0]) || (ERASE_DB == writearr[0]) || (ERASE_50 == writearr[0]) || (ERASE_81 == writearr[0]) ||
        (ERASE_21 == writearr[0]) || (ERASE_5C == writearr[0]) || (ERASE_DC == writearr[0]))

    {
        if (writecnt < 4)
        {
            msg_perr("%s: Address Info Not exit.\n", __func__);
		    return SPI_GENERIC_ERROR;
        }

        if (4 == writecnt)
        {
            cmd = ((writearr[0] << 24) | (0x1 << 22) | (0x1 << 15) | (0x0 << 12));
            MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_CMD_PORT_ADDR, cmd);
            Addr = ((writearr[1] << 16) | (writearr[2] << 8) | (writearr[3]));
        }
        else
        {
            cmd = ((writearr[0] << 24) | (0x1 << 22) | (0x1 << 15) | (0x1 << 12));
            MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_CMD_PORT_ADDR, cmd);
            Addr = ((writearr[1] << 24) | (writearr[2] << 16) | (writearr[3] << 8) | (writearr[4]));
        }

        //msg_perr("Addr  %x %x.\n", Addr, writecnt);

        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_ADDR_PORT_ADDR, Addr);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
            
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_LDATA_PORT_ADDR, 0x1);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
        return 0;
    }

    if ((1 == writecnt) && ((0 == readcnt)))
    {
        cmd = (writearr[0] << 24);
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_CMD_PORT_ADDR, cmd);
       // msg_perr("cmd %x.\n", cmd);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_LDATA_PORT_ADDR, 0x1);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
    }
    else if (writecnt < 6)
    {
        write[0] = writearr[1];
        for (i = 2; i < writecnt; i++)
        {
            write[0] = (write[0] << 8);
            write[0] |= writearr[i];
        }
    }
    else
    {
        memcpy(&write[0], &writearr[1], 4);
        write[1] = writearr[5];
        for (i = 6; i < writecnt; i++)
        {
            write[1] = (write[1] << 8);
            write[1] |= writearr[i];
        }
    }

    if (writecnt > 1)
    {
        Num = ((writecnt - 1) << 3);
        cmd = ((writearr[0] << 24) | (1 << 13) | Num); 
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_CMD_PORT_ADDR, cmd);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_HDATA_PORT_ADDR, write[1]);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_LDATA_PORT_ADDR, write[0]);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
    }

    //msg_perr("write %x %d.\n", writearr[1], writecnt);

    if (readcnt)
    {
        Num = ((readcnt - 1) << 3);
        cmd = ((writearr[0] << 24) | (1 << 13) |Num); 
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_CMD_PORT_ADDR, cmd);
        //msg_perr("cmd %x.\n", cmd);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
        read[0] = MmioRead32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_LDATA_PORT_ADDR);
        read[1] = MmioRead32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_HDATA_PORT_ADDR);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_LDATA_PORT_ADDR, 0x1);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
       // msg_perr("actu read %8x %8x.\n", read[0], read[1]);
    }

    if (0 == readcnt)
    {
        return 0;
    }
    else if (readcnt < 5)
    {
        memcpy(readarr, &read[0], readcnt);
    }
    else
    {
        memcpy((readarr + 4), &read[1], readcnt - 4);
        memcpy(readarr, &read[0], 4);
    }

	return 0;
}

static void spi_pp_cfg(FTE2000_FLASH_MEMMAP *pVirtAddr, struct flashctx *flash, int addr)
{
    //unsigned char writearr[2] = {0x06, 0x00};
    unsigned int CapacityVal[] = {0x400000, 0x800000, 0x1000000, 0x2000000, 0x4000000, 0x8000000};
    unsigned int i = 0x0;

    uint32_t   cmd;

    if (0 == pVirtAddr->size)
    {
        pVirtAddr->size = (flash->chip->total_size * 1024);
        for (i = 0x0; i < sizeof(CapacityVal) / sizeof(CapacityVal[0]); i++)
        {
            if (pVirtAddr->size <= CapacityVal[i])
            {
                MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_FLASH_CAPACITY, i);
                //printf("WILLdump--spi_pp_cfg---0x%08X--\n",i);
                break;
            }
        }

       // MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_CS_SET, 0x5050028);


    }

    if (NULL == pVirtAddr->spi_rd_base_virt)
    {
        pVirtAddr->spi_rd_base_virt = physmap("QSPIRead", SPI_READ_BASE, pVirtAddr->size);
    }

    if (NULL == pVirtAddr->spi_wt_base_virt)
    {
        pVirtAddr->spi_wt_base_virt = physmap("QSPIWrite", SPI_WRITE_BASE, pVirtAddr->size);
    }

    //MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_RD_CFG, 0xe);

    if(flash->in_4ba_mode == true)
    {
        cmd = MmioRead32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_RD_CFG);

        cmd |= 0x13 << 24;
        cmd |= 0x1 << 19;
        cmd |= 0x1 << 3;

        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_RD_CFG, cmd);
    }
    else
    {
        //cmd = MmioRead32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_RD_CFG);
        
        //MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_RD_CFG, cmd);

    }


    if(i > 0x2)
    {
            //writearr[0] = 0xb7;
            //Ft_spi_send_command(flash, 1, 0, writearr, NULL);
#if 0
            


            cmd = 0x02000208;

            cmd |= 0x12 << 24;
            cmd |= 0x1  << 4;
            MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_WR_CFG, cmd);


         

            printf("\nWILLdump--------------spi_pp_cfg---------------------\n"); 
            cmd = MmioRead32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_RD_CFG);
            printf("WILLdump--0x%08X--\n",cmd);
            cmd=0;
            cmd = MmioRead32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_WR_CFG);
            printf("WILLdump--0x%08X--\n",cmd);
            cmd=0;
            printf("\nWILLdump--------------spi_pp_cfg---------------------\n"); 
#endif
    }
    asm volatile ("isb sy":::"cc");
    asm volatile ("dsb sy":::"cc");

    if (flash->address_high_byte != addr)
    {
        //Ft_spi_send_command(flash, 1, 0, writearr, NULL);
       // writearr[0] = flash->chip->wrea_override ? flash->chip->wrea_override : EXADDR;
       // writearr[1] = addr;
       // Ft_spi_send_command(flash, 2, 0, writearr, NULL);
       // flash->address_high_byte = addr;
    }

    return;
}

static int Ft_spi_read(struct flashctx *flash, uint8_t *buf,
			unsigned int start, unsigned int len)
{
    FTE2000_FLASH_MEMMAP *pVirtAddr = get_VirtAddr_from_context(flash);
    if (NULL == pVirtAddr)
		return SPI_GENERIC_ERROR;
        
    spi_pp_cfg(pVirtAddr, flash, (start >> 24));

    if (start > pVirtAddr->size)
    {
        memset (buf, 0xFF, len);
        return 0;
    }

    if ((len & 0x07)||(start & 0x07)) {
//
        // When count is not multiple of 8 bytes, memcpy() function may cause bus error.
        uint8_t    *ptr;
        uint32_t   indx;

        ptr = (uint8_t *)(pVirtAddr->spi_rd_base_virt + start);
        for (indx = 0; indx < len; indx++)
        buf[indx] = ptr[indx];
    } else {
        memcpy (buf, pVirtAddr->spi_rd_base_virt + start, len);
    }

    MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_RD_CFG, 0);
    asm volatile ("isb sy":::"cc");
    asm volatile ("dsb sy":::"cc");
    return 0;
}

static int Ft_spi_write_256(struct flashctx *flash, const uint8_t *buf,
				unsigned int start, unsigned int len)
{
	uint32_t  index, writelen, tmplen;
  
    uintptr_t dest_addr;
    FTE2000_FLASH_MEMMAP *pVirtAddr = get_VirtAddr_from_context(flash);

    uint32_t   cmd;

    spi_pp_cfg(pVirtAddr, flash, (start >> 24));
    if (start > pVirtAddr->size)
    {
        return 0;
    }

    dest_addr = (uintptr_t)pVirtAddr->spi_wt_base_virt + start;
    tmplen = 0;
    while(len)
    {
        writelen = (len > 256) ? 256:len;

        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_CMD_PORT_ADDR, 0x06400000);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_LDATA_PORT_ADDR, 0x1);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");

        cmd = 0x02000208;
        if(flash->in_4ba_mode == true)
        {
            cmd |= 0x12 << 24;
            cmd |= 0x1  << 4;
        }
        //cmd = 0x0;

      

        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_WR_CFG, cmd);

        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
        for(index = tmplen; index < (tmplen + writelen); index += 4) {
            MmioWrite32((dest_addr + index), *(uint32_t *)(buf + index));
        }

        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");

        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_FLASH_FLUSH, 0x1);

        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_WR_CFG, 0x0);
        MmioWrite32(pVirtAddr->spi_ctrl_reg_base_virt + SPI_CMD_PORT_ADDR, 0x0);
        asm volatile ("isb sy":::"cc");
        asm volatile ("dsb sy":::"cc");
        len = len - writelen;
        tmplen +=  writelen;
    }

    return 0;
}

static int Ft_spi_write_aai(struct flashctx *flash, const uint8_t *buf,
				 unsigned int start, unsigned int len)
{
	msg_perr("%s: Not Support.\n", __func__);

	return 0;
}

static int Ft_spi_shutdown(void *data)
{
    FTE2000_FLASH_MEMMAP *pVirtAddr = data;
    if (pVirtAddr->spi_ctrl_reg_base_virt)
    {
        physunmap (pVirtAddr->spi_ctrl_reg_base_virt, SPI_REG_LENGTH);
        pVirtAddr->spi_ctrl_reg_base_virt = NULL;
    }

    if (pVirtAddr->spi_rd_base_virt)
    {
        physunmap (pVirtAddr->spi_rd_base_virt, pVirtAddr->size);
        pVirtAddr->spi_rd_base_virt = NULL;
    }

    if (pVirtAddr->spi_wt_base_virt)
    {
        physunmap (pVirtAddr->spi_wt_base_virt, pVirtAddr->size);
        pVirtAddr->spi_wt_base_virt = NULL;
    }

	return 0;
}

static const struct spi_master spi_master_FTE2000 = {
    .features = SPI_MASTER_4BA,
    .max_data_read = 9,
	.max_data_write = 9,
	.command = Ft_spi_send_command,
	.multicommand = default_spi_send_multicommand,
	.read = Ft_spi_read,
	.write_256 = Ft_spi_write_256,
	.write_aai = Ft_spi_write_aai,
	.shutdown = Ft_spi_shutdown,
};

static void *FtMap(const char *descr, uintptr_t phys_addr, size_t len)
{
    return Ftflash.spi_rd_base_virt;
}

static void FtunMap(void *virt_addr, size_t len)
{
    return;
}

static int FtE2000_spi_init(void)
{
    /*char *arg;

	arg = extract_programmer_param("CPU");
	if (arg) {
		CpuNum = strtol(arg, NULL, 10);
	} else if (arg && !strlen(arg)) {
		msg_perr("Missing argument for CPU.\n");
		free(arg);
		return 1;
	} 

    if (arg)
        free(arg);*/

    //uint32_t cmd=0;
    //char * mem_addr;
    //printf("FtE2000_spi_init\n");
    Ftflash.spi_ctrl_reg_base_virt = physmap("QSPICtrl", SPI_CTRL_REG_BASE, SPI_REG_LENGTH);

    //mem_addr = (char *)Ftflash.spi_ctrl_reg_base_virt;
#if 0
printf("\nWILLdump-----------------------------------\n"); 
    cmd = MmioRead32(mem_addr + 0x0);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;
    cmd = MmioRead32(mem_addr + 0x4);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;

     cmd = MmioRead32(mem_addr + 0x8);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;

     cmd = MmioRead32(mem_addr + 0xc);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;

printf("\nWILLdump-----------------------------------\n");

     cmd = MmioRead32(mem_addr + 0x10);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;

         cmd = MmioRead32(mem_addr + 0x14);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;

         cmd = MmioRead32(mem_addr + 0x18);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;


     cmd = MmioRead32(mem_addr + 0x1c);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;

printf("\nWILLdump-----------------------------------\n");

    cmd = MmioRead32(mem_addr + 0x20);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;

        cmd = MmioRead32(mem_addr + 0x24);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;

        cmd = MmioRead32(mem_addr + 0x28);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;

        cmd = MmioRead32(mem_addr + 0x2C);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;

    cmd = MmioRead32(mem_addr + 0x30);
    printf("WILLdump--0x%08X--\n",cmd);
    cmd=0;


printf("\nWILLdump-----------------------------------\n");

#endif


    return register_spi_master(&spi_master_FTE2000, &Ftflash);
}

const struct programmer_entry programmer_FTE2000 = {
	.name			= "FTE2000",
	.type			= OTHER,
	.devs.note		= "FTE2000.\n",
	.init			= FtE2000_spi_init,
	.map_flash_region	= FtMap,
	.unmap_flash_region	= FtunMap,
	.delay			= internal_delay,
};